#!/usr/bin/python
# -*- coding: UTF-8 -*-

from MiniBotControlLib import MiniBotControlLib

# ------------------------------
# 	1、连接设备 Connect
#   2、扫描模块ID ScanId
#   3、设置关节模块模式 SetMotorMode
#   4、设置关节模块目标速度 SetMotorTargetSpeed
#   5、同步设置多个速度 SetMotorTargetSpeedSync
#   6、获取模块目标速度 GetMotorTargetSpeed
#   7、获取模块当前速度 GetMotorCurrentSpeed
# 	8、断开设备 Disconnect
# ------------------------------

# 声明控制库对象
mlib = MiniBotControlLib("udp")


# *************************
#   1、连接设备
#*************************#
ip = "10.10.100.254"
r = mlib.Connect(ip, 8899)
# 带超时的连接方式
#r = mlib.Connect(ip,8899,2000)
if(r > 0):
    print("连接成功")
else:
    print("连接失败")

# *************************
#   2、扫描模块ID
# *************************
motorIds, motorLocs = mlib.ScanId(0, 50)
print("motorIds", motorIds)
print("motorLocs", motorLocs)


# *************************
#   3、设置关节模块模式
# *************************
input("输入任意字符回车，设置模块为速度模式【请注意选用I模块，其它模块可能会造成损坏】")
for id in motorIds:
    r = mlib.SetMotorMode(id, 1)
    if(r):
        print("模块ID："+str(id) + "设置成功")
    else:
        print("模块ID："+str(id) + "设置失败")


# *************************
#    4、设置关节模块目标速度
# *************************
input("输入任意字符回车，设置关节模块目标速度 50%")
for id in motorIds:
    r = mlib.SetMotorTargetSpeed(id, 50)
    if(r):
        print("模块ID："+str(id) + "设置成功")
    else:
        print("模块ID："+str(id) + "设置失败")


# *************************
#   5、同步设置多个模块目标速度
# *************************
input("输入任意字符回车，同步设置多个速度")
r = mlib.SetMotorTargetSpeedSync(motorIds, [80]*len(motorIds))
if(r):
    print("设置成功")
else:
    print("设置失败")


# *************************
#   6、 获取模块目标速度
# *************************
input("输入任意字符回车， 获取模块目标速度")
for id in motorIds:
    pos = mlib.GetMotorTargetSpeed(id)
    print("模块ID："+str(id) + "速度"+str(pos))


# *************************
#   7、 获取模块当前速度
# *************************
input("输入任意字符回车， 获取模块当前速度")
for id in motorIds:
    pos = mlib.GetMotorCurrentSpeed(id)
    print("模块ID："+str(id) + "速度"+str(pos))


# *************************
#   8、同步设置多个速度【停止】
# *************************
input("输入任意字符回车，同步设置停止")
r = mlib.SetMotorTargetSpeedSync(motorIds, [0]*len(motorIds))
if(r):
    print("设置成功")
else:
    print("设置失败")


# *************************
# 	9、断开设备
# *************************
input("输入任意字符回车，断开连接")
r = mlib.Disconnect()
if(r > 0):
    print("断开成功")
else:
    print("断开失败")
